#include <stdio.h>
#include <string.h>
#include "cmsis_os2.h"
#include "los_task.h"
#include "ohos_init.h"
#include "at32f403a_407.h"
#include "bsp_mpu9250.h"
#include "bsp_beep.h"

/********************************************************************************************************/
extern mpu_data_t mpu_data;
void vTask_IMU()
{
    uint32_t last_tick = osKernelGetTickCount();
    while (1)
    {
        MPU9250_Read_Data_Handle();
        printf("acc XYZ:|	%d	|	%d	|	%d	|\n", mpu_data.accel[0], mpu_data.accel[1], mpu_data.accel[2]);
        printf("gyro XYZ:|	%d	|	%d	|	%d	|\n", mpu_data.gyro[0], mpu_data.gyro[1], mpu_data.gyro[2]);
        printf("mag XYZ:|	%d	|	%d	|	%d	|\n", mpu_data.compass[0], mpu_data.compass[1], mpu_data.compass[2]);
        last_tick = osKernelGetTickCount();
        osDelayUntil(last_tick + 10); // 根据IMU采样频率
    }
}

/**
 * 创建任务
 * @param func 任务函数
 * @param name 任务名
 * @param stack_size 堆栈大小
 * @param priority 优先级
 * @param tid_out 任务ID
 */
static void VTask_Create(osThreadFunc_t func, const char *name, uint32_t stack_size, osPriority_t priority, osThreadId_t *tid_out)
{
    osThreadAttr_t attr;
    attr.name = name;
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = stack_size; // 堆栈大小调大一点
    attr.priority = priority;
    osThreadId_t tid = osThreadNew(func, NULL, &attr);
    if (tid_out)
        *tid_out = tid;
    if (tid == NULL)
    {
        printf(" [%d] User_main_task_create create %s: NG\n", tid, name);
    }
    else
    {
        printf(" [%d]User_main_task_create create %s: OK\n", tid, name);
    }
}
void User_main_task_create(void)
{
    /****************************************************/
    // beep init
    beep_init();
    // MPU_9250 init
    MPU9250_Init();

    VTask_Create(vTask_IMU, "vTask_IMU", 1024 * 20, osPriorityLow, NULL); // 1    +3
}
APP_FEATURE_INIT(User_main_task_create);